package trafficmanager

import (
	"context"
	"errors"
	"math"
	"time"

	"dev.ixmxm.top/rms/boot"
	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/config"
	"dev.ixmxm.top/rms/model"
	"dev.ixmxm.top/rms/utils/angles"
	"dev.ixmxm.top/rms/utils/function"
	"dev.ixmxm.top/rms/utils/log"
	"go.mongodb.org/mongo-driver/bson"
	"go.mongodb.org/mongo-driver/mongo"
	"go.mongodb.org/mongo-driver/mongo/options"
)

//DeleteRobotLocalPaths 清除机器人局部路径规划 和锁点信息
func (planer *Planer) DeleteRobotLocalPaths(ctx context.Context, agvId string, mapName string) codes.Code {
	log.Infof("robot:%s 开始清除机器人局部路径和锁点信息", agvId)
	_, err := planer.db.Collection(ctx, model.LocalPath{}.TableName()).DeleteOne(ctx, bson.M{"sn": agvId, "map_name": mapName})
	if err != nil && !errors.Is(err, mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "delete robot:%s local path error:%s", agvId, err.Error())
		return codes.ErrDatabase.Wrap(err)
	}
	return nil
}

//DeleteRobotLocalPaths 清除机器人局部路径规划(只清除路径和路径锁，不清除锁特殊锁)
func (planer *Planer) ClearRobotLocalPaths(ctx context.Context, agvId string, mapName string) codes.Code {
	log.Infof("robot:%s 开始清除机器人局部路径（不清除锁点信息）", agvId)
	localPath, err := planer.getLocalPathFromDB(ctx, agvId)
	if err != nil {
		return err
	}
	if localPath == nil || localPath.Paths == nil {
		return nil
	}
	localPath.Paths = nil
	dbErr := planer.db.Collection(ctx, model.LocalPath{}.TableName()).FindOneAndReplace(ctx, bson.M{"sn": agvId, "map_name": mapName}, localPath)
	if dbErr != nil && !errors.Is(dbErr.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "清除机器人局部路径规划（不清除锁点信息）失败 robot:%s  error:%s", agvId, dbErr.Err())
		return codes.ErrDatabase.Wrap(dbErr.Err())
	}
	return nil
}
func (planer *Planer) localPathPlanPro(ctx context.Context, robot *model.Robot, mapName string, globalPaths *model.GlobalPath, localRejects map[string]string, saveToDB, needDelReverse bool) (*model.LocalPath, codes.Code, string) {
	deadWayAreas := planer.GetAreaByAreaType(model.AreaTypeDeadWay)
	tideAreas := planer.GetAreaByAreaType(model.AreaTypeTide)
	allowPathMetuexAreas := planer.GetAreaByAreaType(model.AreaTypeAllowPathMutex)
	//获得需要的局部路径
	allNeedLocalPaths := globalPaths.FrontList(robot.Type.LocalPathNodeNum, robot.Type.LocalPathDis, deadWayAreas, tideAreas, allowPathMetuexAreas)
	//获取所有即将前进的，需要旋转的顺序点位

	allApprovedLocalPaths, errGetAll := planer.GetAllLocalPathFromDB(context.Background()) //(ctx)
	if errGetAll != nil {
		return nil, errGetAll, ""
	}
	approvedLocalPaths := &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}
	//计算冲突，批复能批复的路径
	isConflict, isRotateConflict, isStart := false, false, false
	conflictPathId := ""
	conflictAgvId := ""

	pathPointInReject := func(need *model.TrafficPath) (bool, string) {
		for _, point := range need.PathBlocks {
			if _, conflict := localRejects[point]; conflict {
				return true, point
			}
		}
		return false, ""
	}
	//初始化查询所有区域状态

	for index, need := range allNeedLocalPaths {
		// log.Debugf("路径:%d,%s", need.PathId, need.String())
		//判断所需路径是否已经批复给了本机器人
		is_need_in_local_list := false
		for _, approved := range allApprovedLocalPaths {
			if approved.Sn == robot.Account.AgvId {
				for _, item := range approved.Paths {
					if item.PathId == need.PathId && item.Dir == need.Dir && item.RotateType == need.RotateType {
						is_need_in_local_list = true
						break
					}
				}
			}
		}
		//依次处理未批复的路径
		if !is_need_in_local_list {
			//已批复结果检索
			for _, approved := range allApprovedLocalPaths {
				if approved.Sn == robot.Account.AgvId {
					continue
				}
				isConflict, conflictPathId = approved.Conflict(need, planer.getConnectPaths, true, robot)
				if isConflict {
					if conflictPathId == "" {
						log.Warnf("碰撞:车体%s矩形与车体%s所需路径%s矩形碰撞", approved.Sn, robot.Account.AgvId, need.Path.PathId)
					} else {
						log.Warnf("碰撞:车体%s路径%s的矩形与车体%s所需路径%s矩形碰撞", approved.Sn, conflictPathId, robot.Account.AgvId, need.Path.PathId)
					}
				} else {
					//旋转判断车体扫过扇形区域与静止车辆的矩形区域是否相交
					isRotateConflict, isStart = approved.RotateConflictRect(need, robot)
					if isRotateConflict {
						if isStart {
							log.Warnf("碰撞:车体%s矩形和局部路径矩形与车体%s所需节点%s旋转扇形碰撞", approved.Sn, robot.Account.AgvId, need.Path.GetSource().Id)
						} else {
							log.Warnf("碰撞:车体%s矩形和局部路径矩形与车体%s所需节点%s旋转扇形碰撞", approved.Sn, robot.Account.AgvId, need.Path.GetTarget().Id)
						}

					} else {
						isRotateConflict1, isStart1, conflictPath := need.RotateConflictRect(&approved, approved.RotateRadius)
						isRotateConflict = isRotateConflict1
						if isRotateConflict {
							if isStart1 {
								log.Warnf("碰撞:车体%s所需路径%s矩形与车体%s的节点%s旋转扇形碰撞", robot.Account.AgvId, need.Path.PathId, approved.Sn, conflictPath.GetSource().Id)
							} else {
								log.Warnf("碰撞:车体%s所需路径%s矩形与车体%s的节点%s旋转扇形碰撞", robot.Account.AgvId, need.Path.PathId, approved.Sn, conflictPath.GetTarget().Id)
							}

						}
					}
				}
				if isConflict || isRotateConflict {
					if approved.Paths == nil {
						conflictAgvId = approved.Sn
					}
					break
				}
			}
		}
		if isConflict || isRotateConflict {
			break
		}

		if !is_need_in_local_list {
			//点位是否在互斥区域、小车限数区、交互区域
			rejectIds := planer.GetLocalPlanerAreaRejectNodes(ctx, robot, need, allApprovedLocalPaths)
			ids := need.AllBlocks().ToIds()
			pointConflictArea := false
			for _, cover := range ids {
				if _, find := rejectIds[cover]; find {
					// log.Errorf("碰撞: 车体%s所需路径%s矩形与区域冲突", robot.Account.AgvId, need.Path.PathId)
					log.Warnf("碰撞: 车体%s所需路径%s矩形与区域点%s冲突", robot.Account.AgvId, need.Path.PathId, cover)
					connectPaths := planer.getConnectPaths(need.GetTarget())
					for _, p := range connectPaths {
						planer.SetHeatingPathID(60, robot.Account.AgvId, "", []string{p.PathId})
					}
					pointConflictArea = true
					break
				}
			}
			if pointConflictArea {
				break
			}
			//判断当前路径起点不在潮汐但终点在潮汐区域中间点
			if planer.IsTideCanNotGo(need, allNeedLocalPaths, index, allApprovedLocalPaths) {
				break
			}
		}

		//判断所需路径终点是否属于剔除列表
		if !is_need_in_local_list {
			var blockId string
			if isConflict, blockId = pathPointInReject(need); isConflict {
				log.Warnf("robot:%s 路径:%s未批复,路径点:%s属于剔除点", robot.Account.AgvId, need.PathId, blockId)
				planer.heatingPathID(robot.Account.AgvId, "", need.PathId)
				break
			}
		}
		approvedLocalPaths.Paths = append(approvedLocalPaths.Paths, need)
	}

	if len(globalPaths.Paths) >= config.Conf.MinPathLimitNum && len(approvedLocalPaths.Paths) < config.Conf.MinPathLimitNum && len(approvedLocalPaths.Paths) > 0 {
		nodeConnectPaths := planer.getConnectPathsNot(approvedLocalPaths.Paths.Front().GetSource(), approvedLocalPaths.Paths.Front().PathId)
		if len(nodeConnectPaths) == 0 {
			approvedLocalPaths = &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}
		}
	}
	return approvedLocalPaths, nil, conflictAgvId
}
func (planer *Planer) localPathPlan(ctx context.Context, robot *model.Robot, mapName string, globalPaths *model.GlobalPath, localRejects map[string]string, saveToDB, needDelReverse, avoidFlag bool) (*model.LocalPath, codes.Code, string) {
	if saveToDB {
		planer.localPathLocker.Lock()
		defer planer.localPathLocker.Unlock()
	}
	deadWayAreas := planer.GetAreaByAreaType(model.AreaTypeDeadWay)
	tideAreas := planer.GetAreaByAreaType(model.AreaTypeTide)
	allowPathMetuexAreas := planer.GetAreaByAreaType(model.AreaTypeAllowPathMutex)
	//获得需要的局部路径
	allNeedLocalPaths := globalPaths.FrontList(robot.Type.LocalPathNodeNum, robot.Type.LocalPathDis, deadWayAreas, tideAreas, allowPathMetuexAreas)
	robot.LocalPathNum = len(allNeedLocalPaths)

	//顺序拐点

	//计算
	planer.pathLock.Lock()
	planer.agvNeedPaths[robot.Account.AgvId] = globalPaths.Paths.ToIDs()
	for index, p := range globalPaths.Paths {
		if index <= robot.LocalPathNum {
			needSourceConnectPaths := planer.GetConnectPathsInStart(p.GetSource())
			for _, nodeAPath := range needSourceConnectPaths {
				if !blockPathExist(nodeAPath.PathId, planer.agvNeedPaths[robot.Account.AgvId]) {
					planer.agvNeedPaths[robot.Account.AgvId] = append(planer.agvNeedPaths[robot.Account.AgvId], nodeAPath.PathId)
				}
			}
		}
	}
	planer.pathLock.Unlock()

	allApprovedLocalPaths, errGetAll := planer.GetAllLocalPathFromDB(context.Background()) //(ctx)
	if errGetAll != nil {
		return nil, errGetAll, ""
	}

	//允许路线对冲逻辑处理
	allowOrderMutexAreaPath := allNeedLocalPaths.GetAllowPathMuteuxAbout(allApprovedLocalPaths, robot.Account.AgvId)

	approvedLocalPaths := &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}
	//计算冲突，批复能批复的路径
	isConflict, isRotateConflict, isStart := false, false, false
	isAllowLocalPathMetux := false
	conflictPathId := ""
	approvedSuccess := false
	conflictAgvId := ""

	pathPointInReject := func(need *model.TrafficPath) (bool, string) {
		for _, point := range need.PathBlocks {
			if _, conflict := localRejects[point]; conflict {
				return true, point
			}
		}
		return false, ""
	}
	//初始化查询所有区域状态
	// planer.LocalQueryPlanerArea(ctx, robot.Account.AgvId)
	// planer.GetPlanerAreaRejectNodes(ctx, robot, nil)
	callDevice := false
	// GetLocalPath(allNeedLocalPaths, callDevice, approvedSuccess)
	findIndex := -1
	findProcessedIndex := -1

	//判断当前车的全局路径的目标点，是否地堆库和地堆库的前瞻点
	currentNode := globalPaths.Paths.Back().GetTarget()
	//找出该地堆库
	findGroupLocation, relationGroupLocationMaps := planer.FindGroupLocatinByNodeId(currentNode.Id)
	isConfictDesLocation := false
	if findGroupLocation != nil {
		//判断已批复的局部路径,是否在安全范围内，判断哪些点和路径Id不能走
		groundLocationFrontNode := planer.GetNode(findGroupLocation.FrontPointId)
		//判断目标地堆库中是否还存在车
		for _, approvedPath := range allApprovedLocalPaths {
			if approvedPath.Sn == robot.Account.AgvId {
				continue
			}
			isConfictDesLocation = approvedPath.LocationFrontRotateConflictRect(findGroupLocation, groundLocationFrontNode, relationGroupLocationMaps, float64(robot.Type.GetRobotRotateRadius(robot)))
			if isConfictDesLocation {
				//得到
				break
			}
		}
	}
	for index, need := range allNeedLocalPaths {
		// log.Debugf("路径:%d,%s", need.PathId, need.String())
		//判断所需路径是否已经批复给了本机器人
		is_need_in_local_list := false
		for _, approved := range allApprovedLocalPaths {
			if approved.Sn == robot.Account.AgvId {
				for _, item := range approved.Paths {
					if item.PathId == need.PathId && item.Dir == need.Dir && item.RotateType == need.RotateType {
						is_need_in_local_list = true
						findProcessedIndex = index
						break
					}
				}
			}
		}
		// log.Errorf("路径:%d,needBlocks:%v", need.Id, need.AllBlocks())
		//依次处理未批复的路径
		if !is_need_in_local_list {
			//判断当前区域能过？
			isFindBindAllowPathMutex := false
			if need.BindPathMutexAreaId != "" {
				for _, orderAreaObj := range allowOrderMutexAreaPath {
					//
					if orderAreaObj.BindPathMutexAreaId == need.BindPathMutexAreaId {
						isFindBindAllowPathMutex = true
						if orderAreaObj.IsNotGo {
							isAllowLocalPathMetux = true
							log.Errorf("碰撞:车体%s所需局部路径，产生路线局部冲突（可能是局部路径冲突或者静态轮廓冲突）", robot.Account.AgvId)
							//log.Errorf("机器人需要:%s", need.String())
							//todo 所有申请不到的路都增加代价值？
							if orderAreaObj.ConflictMode != model.ConflictNone {
								if orderAreaObj.ConflictTrafficPath != nil {
									log.Errorf("robot:%s 与路线对冲区域：%s robot:%s 产生冲突 类型：%d,路径：%s", robot.Account.AgvId, orderAreaObj.BindPathMutexAreaId, orderAreaObj.ConflictAgvId, orderAreaObj.ConflictMode, orderAreaObj.ConflictTrafficPath.PathId)
									planer.heatingPathID(robot.Account.AgvId, orderAreaObj.ConflictAgvId, orderAreaObj.ConflictTrafficPath.PathId)
									conflictAgvId = orderAreaObj.ConflictAgvId
								}
							}

							break
						}
					}
				}
			}
			if (!isAllowLocalPathMetux && isFindBindAllowPathMutex) || !isFindBindAllowPathMutex {
				//已批复结果检索
				for _, approved := range allApprovedLocalPaths {
					if approved.Sn == robot.Account.AgvId {
						continue
					}
					// log.Infof("其它批复机器人:%s,路径：%v [%v],特殊锁定点:%v", approved.Sn, approved.Paths.ToIDs(), approved.Paths.AllBlocks(), approved.SpecialLocks.ToIds())
					isConflict, conflictPathId = approved.Conflict(need, planer.getConnectPaths, true, robot)
					if isConflict {
						if avoidFlag {
							if conflictPathId == "" {
								log.Warnf("碰撞:车体%s矩形与车体%s所需路径%s矩形碰撞", approved.Sn, robot.Account.AgvId, need.Path.PathId)
							} else {
								log.Warnf("碰撞:车体%s路径%s的矩形与车体%s所需路径%s矩形碰撞", approved.Sn, conflictPathId, robot.Account.AgvId, need.Path.PathId)
							}
						}

					} else {
						//旋转判断车体扫过扇形区域与静止车辆的矩形区域是否相交
						isRotateConflict, isStart = approved.RotateConflictRect(need, robot)
						if isRotateConflict {
							if isStart {
								log.Warnf("碰撞:车体%s矩形和局部路径矩形与车体%s所需节点%s旋转扇形碰撞", approved.Sn, robot.Account.AgvId, need.Path.GetSource().Id)
							} else {
								log.Warnf("碰撞:车体%s矩形和局部路径矩形与车体%s所需节点%s旋转扇形碰撞", approved.Sn, robot.Account.AgvId, need.Path.GetTarget().Id)
							}

						} else {
							isRotateConflict1, isStart1, conflictPath := need.RotateConflictRect(&approved, approved.RotateRadius)
							isRotateConflict = isRotateConflict1
							if isRotateConflict {
								if isStart1 {
									log.Warnf("碰撞:车体%s所需路径%s矩形与车体%s的节点%s旋转扇形碰撞", robot.Account.AgvId, need.Path.PathId, approved.Sn, conflictPath.GetSource().Id)
								} else {
									log.Warnf("碰撞:车体%s所需路径%s矩形与车体%s的节点%s旋转扇形碰撞", robot.Account.AgvId, need.Path.PathId, approved.Sn, conflictPath.GetTarget().Id)
								}

							}
						}
					}
					if isConflict || isRotateConflict {
						//todo 所有申请不到的路都增加代价值？
						gotAgv := false
						if approved.Paths == nil {
							conflictAgvId = approved.Sn
							gotAgv = true
						} else {
							if gPaths, errGetG := planer.GetRobotGlobalPaths(context.Background(), approved.Sn); errGetG == nil &&
								gPaths != nil && gPaths.Paths != nil && blockPathExist(need.PathId, gPaths.Paths.ToIDs()) {
								conflictAgvId = approved.Sn
								gotAgv = true
							}
						}
						if gotAgv {
							globalPaths.BlockStart = need.PathId
							if avoidFlag {
								planer.heatingPathID(robot.Account.AgvId, approved.Sn, need.PathId)
							}
						}
						break
					}
				}
			}
		}
		if isAllowLocalPathMetux || isConflict || isRotateConflict {
			break
		}

		if !is_need_in_local_list {
			//点位是否在互斥区域、小车限数区、交互区域
			rejectIds := planer.GetLocalPlanerAreaRejectNodes(ctx, robot, need, allApprovedLocalPaths)
			ids := need.AllBlocks().ToIds()
			pointConflictArea := false
			for _, cover := range ids {
				if _, find := rejectIds[cover]; find {
					// log.Errorf("碰撞: 车体%s所需路径%s矩形与区域冲突", robot.Account.AgvId, need.Path.PathId)
					log.Warnf("碰撞: 车体%s所需路径%s矩形与区域点%s冲突", robot.Account.AgvId, need.Path.PathId, cover)
					connectPaths := planer.getConnectPaths(need.GetTarget())
					for _, p := range connectPaths {
						planer.SetHeatingPathID(60, robot.Account.AgvId, "", []string{p.PathId})
					}
					pointConflictArea = true
					break
				}
			}
			if pointConflictArea {
				break
			}
			//判断当前路径起点不在潮汐但终点在潮汐区域中间点
			if planer.IsTideCanNotGo(need, allNeedLocalPaths, index, allApprovedLocalPaths) {
				break
			}

			if isConfictDesLocation {
				//判断已批复的局部路径,是否在安全范围内，判断哪些点和路径Id不能走
				groundLocationFrontNode := planer.GetNode(findGroupLocation.FrontPointId)
				//判断自己当前规划的局部路径是否在目标棒棒糖内部
				isInGroup := need.LocationFrontRotateConflictRect(findGroupLocation, groundLocationFrontNode, relationGroupLocationMaps, float64(robot.Type.GetRobotRotateRadius(robot)), planer.GetNode)
				if isInGroup {
					break
				}
			}

			//判断当前区域不在自动们区域，下个点在自动们区域，申请进去区域
			isGoIn, doorArea := planer.IsGoToDoorArea(need, globalPaths.Target)
			if isGoIn {
				if doorArea != nil && doorArea.IsApply == 0 {
					// if index == 0 {
					//请求进入区域
					if errApply := planer.devManager.OperatorApplyArea(ctx, *doorArea, robot.Account.AgvId, planer.m.MapName, model.OperatorOpen); errApply != nil {
						log.Infof("robot:%s,（进门区域:%s，目标点:%s在区域中，路径编号：%d）,请求进入!!!（", robot.Account.AgvId, doorArea.Id, need.GetTarget().Id, need.Id)
						isConflict = true
						callDevice = true
						break
					}
				}
				// }
				// else {
				// 	log.Infof("robot:%s,（进门区域:%s，当前还存在未走完的局部路径，目标点:%s在区域中，路径编号：%d）!!!（", robot.Account.AgvId, doorArea.Id, need.GetTarget().Id, need.Id)
				// 	isConflict = true
				// 	callDevice = true
				// 	break
				// }
			}
		}

		//判断所需路径终点是否属于剔除列表
		if !is_need_in_local_list {
			var blockId string
			if isConflict, blockId = pathPointInReject(need); isConflict {
				log.Warnf("robot:%s 路径:%s未批复,路径点:%s属于剔除点", robot.Account.AgvId, need.PathId, blockId)
				planer.heatingPathID(robot.Account.AgvId, "", need.PathId)
				break
			}
		}
		if !is_need_in_local_list {
			//设备操作自动们和电梯
			if need.GetTarget().DeviceID != "" && (need.GetTarget().Type == model.NodeElevator || need.GetTarget().Type == model.NodeAutoDoor) {
				// if errOperate := planer.devManager.Open(need.GetTarget().DeviceID, robot.Account.AgvId, planer.m.MapName); errOperate != nil {
				// 	log.Infof("节点:%s为设备节点，设备未开启,路径:%s未批复", need.GetTarget().Id, need.PathId)
				// 	isConflict = true
				// 	callDevice = true
				// 	break
				// }
				//判断当前执行任务第一条是自动门
				if index == 0 {
					if errOperate := planer.devManager.OperatorAll(ctx, need.GetTarget(), robot.Account.AgvId, planer.m.MapName, model.OperatorOpen); errOperate != nil {
						log.Infof("节点:%s为设备节点，设备未开启,路径:%d未批复", need.GetTarget().Id, need.Id)
						isConflict = true
						callDevice = true
						break
					}
				} else if index <= config.Conf.PathNumApplyDoor {
					//请求开门
					planer.devManager.OperatorAll(ctx, need.GetTarget(), robot.Account.AgvId, planer.m.MapName, model.OperatorOpen)
					log.Infof("robot:%s,（进自动门或者电梯，设备点:%s,路径编号：%d）,请求开门!!!（", robot.Account.AgvId, need.GetTarget().Id, need.Id)
					isConflict = true
					callDevice = true
					break
				} else {
					log.Infof("robot:%s,（进自动门或者电梯，设备点:%s,路径编号：%d）,请求开门,路径相隔太远不通过!!!（", robot.Account.AgvId, need.GetTarget().Id, need.Id)
					isConflict = true
					callDevice = true
					break
				}

			} else if need.GetSource().DeviceID != "" && need.GetSource().Type == model.NodeElevator {
				if index == 0 {
					if errOperate := planer.devManager.OperatorAll(ctx, need.GetSource(), robot.Account.AgvId, planer.m.MapName, model.OperatorOutOpen); errOperate != nil {
						log.Infof("节点:%s为设备节点，电梯出门未开启,路径:%s未批复", need.GetSource().Id, need.PathId)
						isConflict = true
						callDevice = true
						break
					}
				} else {
					planer.devManager.OperatorAll(ctx, need.GetSource(), robot.Account.AgvId, planer.m.MapName, model.OperatorOutOpen)
					log.Infof("robot:%s,（出电梯，设备点:%s,路径编号：%d）,请求开门!!!（", robot.Account.AgvId, need.GetSource().Id, need.Id)
					isConflict = true
					callDevice = true
					break

				}
			}
			//称重平台
			if need.GetTarget().DeviceID != "" && need.GetTarget().Type == model.NodePlatformWeight {
				if len(allNeedLocalPaths) > 1 {
					isConflict = true
					callDevice = true
					break
				}
				if errOperate := planer.devManager.OperatorAll(ctx, need.GetTarget(), robot.Account.AgvId, planer.m.MapName, model.OperatorWeighPick); errOperate != nil {
					log.Infof("节点:%s为称重平台节点，设备未开启,路径:%d未批复", need.GetTarget().Id, need.Id)
					isConflict = true
					callDevice = true
					break
				}

			}
			//输送线----------------------------
			if need.GetTarget().DeviceID != "" && need.GetTarget().Type == model.NodeConveyorLine {
				if index != 0 && ((index + 1) > config.Conf.PathNumApplyConveyorLine) {
					break
				}
				// if len(allNeedLocalPaths) > 1 {
				// 	break
				// }
				if errOperate := planer.devManager.OperatorAll(ctx, need.GetTarget(), robot.Account.AgvId, planer.m.MapName, model.OperatorOpen); errOperate != nil {
					log.Infof("节点:%s为设备节点，设备未开启,路径:%d未批复", need.GetTarget().Id, need.Id)
					isConflict = true
					callDevice = true
					break
				}

			}
		}
		approvedSuccess = true
		approvedLocalPaths.Paths = append(approvedLocalPaths.Paths, need)
		findIndex = index
	}
	//获得已经生成待下发的最后一条路径
	// log.Infof("robot:%s ,可达路径下标:%d 已批复路径下标:%d allNeedLocalPaths:%d", robot.Account.AgvId, findIndex, findProcessedIndex, len(allNeedLocalPaths))
	if len(approvedLocalPaths.Paths) > 0 && findIndex > findProcessedIndex {
		lastApprovedLocalPath := approvedLocalPaths.Paths[len(approvedLocalPaths.Paths)-1]
		// lastNeedLocalPath := allNeedLocalPaths[len(allNeedLocalPaths)-1]
		findBindIndex := -1
		//找出
		if lastApprovedLocalPath.BindDeadWayId != "" && findIndex != (len(allNeedLocalPaths)-1) {
			//先判断最后一条批复路径的目标点是否在死胡同
			isIn, _ := planer.IsInDeadArea(lastApprovedLocalPath.GetTarget().Id)
			// log.Infof("robot:%s 当前最后批复局部路径起点在死胡同，终点:%v", robot.Account.AgvId, isIn)
			if isIn {
				//删除不必要的区域
				for j := len(approvedLocalPaths.Paths) - 1; j >= 0; j-- {
					if approvedLocalPaths.Paths[j].BindDeadWayId == "" {
						findBindIndex = j - 1
						break
					}
				}
			} else {
				findBindIndex = findIndex
			}
		} else if lastApprovedLocalPath.BindDeadWayId == "" && findIndex != (len(allNeedLocalPaths)-1) && allNeedLocalPaths[findIndex+1].BindDeadWayId != "" {
			findBindIndex = len(approvedLocalPaths.Paths) - 2
			log.Infof("robot:%s 当前最后批复局部路径不在死胡同,下一条局部路径在死胡同,批复下标：%d", robot.Account.AgvId, findBindIndex)
		} else {
			findBindIndex = findIndex
		}

		if findBindIndex > -1 {
			log.Infof("robot:%s ,加上死胡同最大可走下标:%d 已批复最大路径:%d", robot.Account.AgvId, findBindIndex, findProcessedIndex)
		}

		if findBindIndex == -1 && findProcessedIndex == -1 {
			approvedSuccess = false
			approvedLocalPaths.Paths = append(approvedLocalPaths.Paths[:0])
		} else if findProcessedIndex >= -1 && findBindIndex >= findProcessedIndex {
			approvedLocalPaths.Paths = append(approvedLocalPaths.Paths[:findBindIndex+1])
		}

	}

	// if len(globalPaths.Paths) > 1 && len(approvedLocalPaths.Paths) == 1 {
	// 	nodeConnectPaths := planer.getConnectPathsNot(approvedLocalPaths.Paths.Front().GetSource(), approvedLocalPaths.Paths.Front().PathId)
	// 	if len(nodeConnectPaths) == 0 {
	// 		approvedSuccess = false
	// 		// planer.heatingPathID(approvedLocalPaths.Sn, approvedLocalPaths.Paths.ToIDs()...)
	// 		approvedLocalPaths = &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}
	// 	}
	// }
	//
	log.Warnf("robot:%s ,approvedSuccess:%v ,approvedLocalPaths.Paths size:%d", robot.Account.AgvId, approvedSuccess, len(approvedLocalPaths.Paths))
	if findProcessedIndex < 0 {
		if len(globalPaths.Paths) >= config.Conf.MinPathLimitNum && len(approvedLocalPaths.Paths) < config.Conf.MinPathLimitNum && len(approvedLocalPaths.Paths) > 0 {
			// isNotCalNext := false
			nodeConnectPaths := planer.getConnectPathsNot(approvedLocalPaths.Paths.Front().GetSource(), approvedLocalPaths.Paths.Front().PathId)
			if len(nodeConnectPaths) == 0 {
				// isNotCalNext = true
				approvedSuccess = false
				planer.heatingPathID(robot.Account.AgvId, approvedLocalPaths.Sn, approvedLocalPaths.Paths.ToIDs()...)
				approvedLocalPaths = &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}
			}
			// if !isNotCalNext && planer.IsConnectPathsNextIsStation(approvedLocalPaths.Paths.Front().GetSource()) {
			// 	isNotCalNext = true
			// 	approvedSuccess = false
			// 	planer.heatingPathID(robot.Account.AgvId, approvedLocalPaths.Sn, approvedLocalPaths.Paths.ToIDs()...)
			// 	approvedLocalPaths = &model.LocalPath{Sn: robot.Account.AgvId, MapName: mapName, Paths: model.TrafficPaths{}}
			// }

		}
	}

	if approvedSuccess {
		agv_node := approvedLocalPaths.Paths.Front().GetSource()
		thetaOPRad := angles.Deg2Rad(float64(robot.Status.Angle))
		rect := robot.Type.GotLockRectangle(robot, thetaOPRad, float64(agv_node.X), float64(agv_node.Y), 0, agv_node.Type, true, false)
		rotate_radius := robot.Type.GetRobotRotateRadius(robot)
		robotr := robot.Type.GetRobotBackRotateRadius(robot)
		approvedLocalPaths.CurrentNode = agv_node
		approvedLocalPaths.RotateRadius = rotate_radius
		approvedLocalPaths.BackRadius = robotr
		approvedLocalPaths.NodeRect = rect
		approvedLocalPaths.RobotAng = float64(robot.Status.Angle)
		if saveToDB {
			_ = planer.saveLocalPathToDB(ctx, approvedLocalPaths, false, false, robot)
		}
	}

	commit := func() {
		log.Debugf("robot:%s批复路径:%s", robot.Account.AgvId, approvedLocalPaths.Paths.String())
		planer.removePathIDConflictMap(approvedLocalPaths.Paths.ToIDs()...)
	}
	rollback := func() {
	}
	boot.TryAddTx(ctx, commit, rollback)
	if len(approvedLocalPaths.Paths) == 0 {
		if callDevice && conflictAgvId == "" {
			return approvedLocalPaths, codes.ErrCallDevice, conflictAgvId
		}
		return approvedLocalPaths, nil, conflictAgvId
	}
	return approvedLocalPaths, nil, conflictAgvId
}

func (planer *Planer) GetAllLocalPathFromDB(ctx context.Context) ([]model.LocalPath, codes.Code) {
	dbRes, err := planer.db.Collection(ctx, model.LocalPath{}.TableName()).Find(ctx, bson.M{"map_name": planer.m.MapName})
	if err != nil {
		log.ErrorfWithContext(ctx, "get all local path error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	res := make([]model.LocalPath, 0)
	if errBind := dbRes.All(ctx, &res); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all local path error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return res, nil
}
func (planer *Planer) GetLocalPathByAgvIdFromDB(ctx context.Context, agvId string) (*model.LocalPath, codes.Code) {
	dbRes := planer.db.Collection(ctx, model.LocalPath{}.TableName()).FindOne(ctx, bson.M{"map_name": planer.m.MapName, "sn": agvId})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get robot:%s local path error:%s", agvId, dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		//log.Errorf("can not find robot running:%s task",agvID)
		return nil, nil
	}
	localPath := new(model.LocalPath)

	if errBind := dbRes.Decode(localPath); errBind != nil {
		log.ErrorfWithContext(ctx, "bind all local path error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return localPath, nil
}

//获取所有锁定的点位
func (planer *Planer) getAllLocalSiteFromDB(ctx context.Context) (model.PathLocks, codes.Code) {
	localPaths, err := planer.GetAllLocalPathFromDB(ctx)
	if err != nil {
		return nil, codes.ErrDatabase.Wrap(err)
	}
	siteLocks := make(model.PathLocks, 0)
	for _, localPath := range localPaths {
		if len(localPath.Paths) > 0 {
			for _, path := range localPath.Paths {
				pathLocks := path.AllBlocks()
				for _, pathLock := range pathLocks {
					siteLocks = append(siteLocks, pathLock)
				}
			}
		} else {
			for _, specialLock := range localPath.SpecialLocks {
				siteLocks = append(siteLocks, specialLock)
			}
		}
	}
	return siteLocks, nil
}

func (planer *Planer) GetAllLocalSiteFromDB(ctx context.Context) (model.PathLocks, codes.Code) {
	localPaths, err := planer.GetAllLocalPathFromDB(ctx)
	if err != nil {
		return nil, codes.ErrDatabase.Wrap(err)
	}
	siteLocks := make(model.PathLocks, 0)
	for _, localPath := range localPaths {
		if len(localPath.Paths) > 0 {
			for _, path := range localPath.Paths {
				pathLocks := path.AllBlocks()
				for _, pathLock := range pathLocks {
					siteLocks = append(siteLocks, pathLock)
				}
			}
		} else {
			for _, specialLock := range localPath.SpecialLocks {
				siteLocks = append(siteLocks, specialLock)
			}
		}
	}
	return siteLocks, nil
}

//获得所有其它车锁定的点位
func (planer *Planer) getAllOtherLocalSiteFromDB(ctx context.Context, currentAgvId string) (model.PathLocks, codes.Code) {
	//锁住
	localPaths, err := planer.GetAllLocalPathFromDB(ctx)
	if err != nil {
		return nil, codes.ErrDatabase.Wrap(err)
	}
	siteLocks := make(model.PathLocks, 0)
	for _, localPath := range localPaths {
		if localPath.Sn == currentAgvId {
			continue
		}
		if len(localPath.Paths) > 0 {
			for _, path := range localPath.Paths {
				pathLocks := path.AllBlocks()
				for _, pathLock := range pathLocks {
					siteLocks = append(siteLocks, pathLock)
				}
			}
		} else {
			for _, specialLock := range localPath.SpecialLocks {
				siteLocks = append(siteLocks, specialLock)
			}
		}
	}
	return siteLocks, nil
}
func (planer *Planer) saveLocalPathToDB(ctx context.Context, localPath *model.LocalPath, isOccupyElevetor, isDoOnline bool, robot *model.Robot) codes.Code {
	//todo 临时，释放设备
	oldLocalPath, errGet := planer.getRobotLocalPaths(ctx, localPath.Sn)
	if errGet != nil && !errors.Is(errGet, codes.ErrLocalPathNotFound) {
		return errGet
	}
	closeDev := func(node model.Node, isOccupyElevetor bool) {
		if node.DeviceID != "" {
			if node.Type == model.NodeAutoDoor {
				if isDoOnline {
					return
				}
				planer.devManager.OperatorAll(ctx, node, localPath.Sn, planer.m.MapName, model.OperatorClose)
				// planer.devManager.Close(node.DeviceID, localPath.Sn, planer.m.MapName)
			} else if node.Type == model.NodeElevator {
				if isOccupyElevetor {
					return
				}
				if len(localPath.Paths) == 0 && localPath.CurrentNode.Type == model.NodeElevator {
					log.Infof("进电梯不需要申请关门")
				} else {
					planer.devManager.OperatorAll(ctx, node, localPath.Sn, planer.m.MapName, model.OperatorClose)
					// planer.devManager.Close(node.DeviceID, localPath.Sn, planer.m.MapName)
					planer.devManager.ModifyElevatorOccupyToDB(ctx, node.DeviceID, "")
				}
			}
		}
		// log.Warnf("close node %s  current = %s", node.Id, localPath.CurrentNode.Id)
		if node.Type == model.NodeSwitch && localPath.CurrentNode.Type != model.NodeSwitch {
			planer.devManager.ModifyAreaSwitchOccupyToDB(ctx, planer.m.MapName, node.Id, "")
		}
	}
	nodes := make([]model.Node, 0) //申请到的局部路径点
	for index, p := range localPath.Paths {
		if index == 0 {
			nodes = append(nodes, p.GetSource())
		}
		nodes = append(nodes, p.GetTarget())
	}
	curNodes := make([]model.Node, 0) //当前正在用的局部路径点
	if oldLocalPath != nil {
		for index, p := range oldLocalPath.Paths {
			if index == 0 {
				curNodes = append(curNodes, p.GetSource())
			}
			curNodes = append(curNodes, p.GetTarget())
		}
	}

	//若当前正在用的点不再使用，即已经释放
	for _, cur := range curNodes {
		find := false
		for _, n := range nodes {
			if n.Id == cur.Id {
				find = true
				break
			}
		}
		if !find {
			closeDev(cur, isOccupyElevetor)
		}
	}

	if len(localPath.Paths.AllBlocks().ToIds()) == 0 && len(localPath.SpecialLocks.ToIds()) == 0 {
		log.ErrorfWithContext(ctx, "no locker")
	}
	//todo 整合函数
	//释放申请的路径
	if oldLocalPath != nil && oldLocalPath.Paths != nil {
		for _, old := range oldLocalPath.Paths {
			find := false
			for _, item := range localPath.Paths {
				if old.PathId == item.PathId {
					find = true
					break
				}
			}
			if !find {
				planer.removePathIDConflictMap(old.PathId)
			}
		}
	}
	dbRes := planer.db.Collection(ctx, model.LocalPath{}.TableName()).FindOneAndReplace(ctx, bson.M{"sn": localPath.Sn}, localPath, &options.FindOneAndReplaceOptions{Upsert: function.BoolPtr(true)})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "save local path to db error:%s", dbRes.Err().Error())
		return codes.ErrDatabase.Wrap(dbRes.Err())
	}
	// planer.GetPlanerAreaRejectNodes(ctx, robot, nil)
	return nil
}

func (planer *Planer) getRobotLocalPaths(ctx context.Context, sn string) (*model.LocalPath, codes.Code) {
	dbRes := planer.db.Collection(ctx, model.LocalPath{}.TableName()).FindOne(ctx, bson.M{"sn": sn})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get robot local path error:%s", dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		return nil, codes.ErrLocalPathNotFound
	}
	res := new(model.LocalPath)
	if errBind := dbRes.Decode(res); errBind != nil {
		log.ErrorfWithContext(ctx, "bind robot local path error:%s", errBind.Error())
		return nil, codes.ErrDatabase.Wrap(errBind)
	}
	return res, nil
}

func (planer *Planer) getLocalPathFromDB(ctx context.Context, sn string) (*model.LocalPath, codes.Code) {
	dbRes := planer.db.Collection(ctx, model.LocalPath{}.TableName()).FindOne(ctx, bson.M{"sn": sn})
	if dbRes.Err() != nil && !errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		log.ErrorfWithContext(ctx, "get local path from db error:%s", dbRes.Err().Error())
		return nil, codes.ErrDatabase.Wrap(dbRes.Err())
	}
	if dbRes.Err() != nil && errors.Is(dbRes.Err(), mongo.ErrNoDocuments) {
		return nil, codes.ErrLocalPathNotFound
	}
	res := new(model.LocalPath)
	if err := dbRes.Decode(res); err != nil {
		log.ErrorfWithContext(ctx, "bind local path error:%s", err.Error())
		return nil, codes.ErrDatabase.Wrap(err)
	}
	return res, nil
}

func (planer *Planer) heatingPathID(need_sn, approved_sn string, ids ...string) {
	planer.conflictPathIDMapLock.Lock()
	defer planer.conflictPathIDMapLock.Unlock()
	if planer.devManager.SystemConfig.TrafficCfg.UseReroute {
		for _, item := range ids {
			cost, find := planer.conflictPathIDMap[item]
			if !find {
				planer.conflictPathIDMap[item] = struct {
					heat              float64
					sn_need           string
					sn_have           string
					startConflictTime time.Time //开始冲突时间
					lastConflictTime  time.Time
				}{heat: 0.0, sn_need: need_sn, sn_have: approved_sn, startConflictTime: time.Now(), lastConflictTime: time.Now()}
				return
			}
			// log.Warnf("time=%d path=%s", time.Now().Sub(cost.startConflictTime)/time.Second, item)
			lowTime := planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.WaitTime + planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.CostSwitchTime
			if time.Now().Sub(cost.startConflictTime) < time.Duration(int64(planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.WaitTime))*time.Second {

			} else if time.Now().Sub(cost.startConflictTime) < time.Duration(int64(lowTime))*time.Second {
				cost.heat += planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.LowCostSwitchSlope
			} else {
				cost.heat += planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.HighCostSwitchSlope
			}
			cost.lastConflictTime = time.Now()
			planer.conflictPathIDMap[item] = cost
		}
	}

}
func (planer *Planer) SetHeatingPathID(value float64, need_sn, approved_sn string, ids []string) {
	planer.conflictPathIDMapLock.Lock()
	defer planer.conflictPathIDMapLock.Unlock()
	if planer.devManager.SystemConfig.TrafficCfg.UseReroute {
		for _, item := range ids {
			cost, find := planer.conflictPathIDMap[item]
			if !find {
				planer.conflictPathIDMap[item] = struct {
					heat              float64
					sn_need           string
					sn_have           string
					startConflictTime time.Time //开始冲突时间
					lastConflictTime  time.Time
				}{heat: value, sn_need: need_sn, sn_have: approved_sn, startConflictTime: time.Now(), lastConflictTime: time.Now()}
				continue
			} else {
				cost.heat = math.Max(value, cost.heat)
			}
			lowTime := planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.WaitTime + planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.CostSwitchTime
			if time.Now().Sub(cost.startConflictTime) < time.Duration(int64(planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.WaitTime))*time.Second {

			} else if time.Now().Sub(cost.startConflictTime) < time.Duration(int64(lowTime))*time.Second {
				cost.heat += planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.LowCostSwitchSlope
			} else {
				cost.heat += planer.devManager.SystemConfig.TrafficCfg.RerouteCfg.HighCostSwitchSlope
			}
			cost.lastConflictTime = time.Now()
			planer.conflictPathIDMap[item] = cost
		}
	}
}

func (planer *Planer) removePathIDHeat(ids []string) {
	planer.conflictPathIDMapLock.Lock()
	defer planer.conflictPathIDMapLock.Unlock()
	for _, item := range ids {
		if lock, find := planer.conflictPathIDMap[item]; find && lock.sn_need == "" {
			delete(planer.conflictPathIDMap, item)
		}
	}
}
func blockPathExist(p string, list []string) bool {
	for _, l := range list {
		if p == l {
			return true
		}
	}
	return false
}
func (planer *Planer) getConflictPathIDHeat(sn string, pathId string) float64 {

	planer.conflictPathIDMapLock.Lock()
	defer planer.conflictPathIDMapLock.Unlock()
	if len(planer.conflictPathIDMap) == 0 {
		return 0.0
	}
	cost, find := planer.conflictPathIDMap[pathId]
	if !find {
		//log.Infof("getConflictLockHeat:sn%s id:%d,heat:%f[not match]", sn, lock.Id, 0.0)
		return 0.0
	}
	if cost.sn_have == sn {
		// if cost.static {
		// 	delete(planer.conflictPathIDMap, pathId)
		// }
		//log.Infof("getConflictLockHeat:sn%s id:%d,heat:%f[sn match]", sn, l.Id, 0.0)
		return 0.0
	}
	if time.Now().Sub(cost.lastConflictTime) > 2*time.Second && cost.sn_need == "" {
		//如果长时间没冲突
		//log.Infof("getConflictLockHeat:sn%s id:%d,heat:%f[long time]", sn, l.Id, 0.0)
		delete(planer.conflictPathIDMap, pathId)
		return 0.0
	}
	// if time.Now().Sub(cost.lastConflictTime) > 5*time.Second {
	if cost.sn_have != "" {
		if lPath, errGet := planer.getRobotLocalPaths(context.Background(), cost.sn_have); errGet == nil && lPath != nil && lPath.Paths != nil {
			if gPaths, errGetG := planer.GetRobotGlobalPaths(context.Background(), cost.sn_have); errGetG == nil &&
				gPaths != nil && gPaths.Paths != nil &&
				blockPathExist(pathId, gPaths.Paths.ToIDs()) {

			} else {
				delete(planer.conflictPathIDMap, pathId)
				return 0.0
			}
		}
	}

	if time.Now().Sub(cost.lastConflictTime) > 2*time.Second && cost.sn_need != "" {
		planer.pathLock.Lock()
		paths := planer.agvNeedPaths[cost.sn_need]
		planer.pathLock.Unlock()
		if len(paths) != 0 {
			if blockPathExist(pathId, paths) {
				return cost.heat
			}
		}
		//如果长时间没冲突
		//log.Infof("getConflictLockHeat:sn%s id:%d,heat:%f[long time]", sn, l.Id, 0.0)
		delete(planer.conflictPathIDMap, pathId)
		return 0.0
	}
	// }
	//log.Infof("getConflictLockHeat:sn%s id:%d,heat:%f", sn, l.Id, l.Heat)
	// log.Infof("pathId%s cost=%f", pathId, cost.heat)
	return cost.heat
}

func (planer *Planer) removePathIDConflictMap(ids ...string) {
	planer.conflictPathIDMapLock.Lock()
	defer planer.conflictPathIDMapLock.Unlock()
	for _, item := range ids {
		//log.Debugf("remove path id:%d", item)
		delete(planer.conflictPathIDMap, item)
	}
}

//reduceLocalPathLocks 根据距离终点的阈值，减少锁定点
func (planer *Planer) reduceLocksRelTarget(robot *model.Robot, ctx context.Context, target model.Node, locks model.PathLocks, robot_theta, next_path_theta float64, robotType model.RobotType) model.PathLocks {
	sBlocks := make(model.PathLocks, 0)

	robotR := robotType.GetRobotRotateRadius(robot)
	robotr := robotType.GetRobotBackRotateRadius(robot)
	wHalf := robotType.GetRobotHalfWidth(robot)

	inRectangle := func(node model.Node, theta, x, y, l, robotHead, robotTail, width float64) bool {
		lHead := l + robotHead //车头：路径长度/2+头部距离
		lTail := l + robotTail //车尾：路径长度/2+尾部距离
		x1, y1 := x+lHead*math.Cos(theta)-width*math.Sin(theta), y+lHead*math.Sin(theta)+width*math.Cos(theta)
		x2, y2 := x+(-lTail*math.Cos(theta)-width*math.Sin(theta)), y+(-lTail*math.Sin(theta)+width*math.Cos(theta))
		x3, y3 := x+(-lTail*math.Cos(theta)+width*math.Sin(theta)), y+(-lTail*math.Sin(theta)-width*math.Cos(theta))
		x4, y4 := x+lHead*math.Cos(theta)+width*math.Sin(theta), y+lHead*math.Sin(theta)-width*math.Cos(theta)

		getCross := func(p1x, p1y, p2x, p2y float64) float64 {
			return (p2x-p1x)*(float64(node.Y)-p1y) - (float64(node.X)-p1x)*(p2y-p1y)
		}
		return getCross(x3, y3, x2, y2)*getCross(x1, y1, x4, y4) >= 0 && getCross(x2, y2, x1, y1)*getCross(x4, y4, x3, y3) >= 0
	}

	for _, lock := range locks {
		lockNode := planer.GetNode(lock)
		inFlg := false
		if inRectangle(lockNode, robot_theta, float64(target.X), float64(target.Y), 0, robotR, robotr, wHalf) {
			sBlocks.Add(lock)
			inFlg = true
		}
		if inRectangle(lockNode, next_path_theta, float64(target.X), float64(target.Y), 0, robotR, robotr, wHalf) {
			sBlocks.Add(lock)
			inFlg = true
		}
		if inFlg {
			sBlocks.Adds(lockNode.AssociatedBlocks)
		}
	}
	return sBlocks
}
